// Code in autofunction that is not used for whatever reason (doesn't work well, there's a better approach, etc)
// They are put in here for reference and the comments are not updated

// ----------------------------- Drive -----------------------------------------------
void moveForwardT(float distance) { // Moves Forward By Ticks
  int prevEncL; int prevEncR;
  int moveamount = distance;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = minMovePower + 10;
	int wheelRNormal = minMovePower + 10;
  while (abs(moveamount - SensorValue[LeftEncoder]) > driveThreshold && abs(moveamount - SensorValue[RightEncoder]) > driveThreshold) {
    driveState = 1;
    bob += 1;
    if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
      wheelLNormal += 1;
    }
    if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
      wheelRNormal += 1;
    }
    if ((distance - SensorValue[LeftEncoder]) < 0) {
      wheelLeftPower = -wheelLNormal;
    }
    else {
      wheelLeftPower = wheelLNormal;
    }
    if ((distance - SensorValue[RightEncoder]) < 0) {
      wheelRightPower = -wheelRNormal;
    }
    else {
      wheelRightPower = wheelRNormal;
    }
    prevEncL = SensorValue[LeftEncoder];
    prevEncR = SensorValue[RightEncoder];
    wait1Msec(10);
  }
  wheelLeftPower = 0;
  wheelRightPower = 0;
  driveState = 3;
}

// --------------------------------- Line ------------------------------------
void moveToLine(char sensor, int direction, int value, int power = 3, bool towards = true) {
  int prevEncL; int prevEncR;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = minMovePower + power;
	int wheelRNormal = minMovePower + power;
	if (towards) {
	 while (SensorValue[sensor] > value) {
	   driveState = 5;
	   /*if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
       wheelLNormal += 1;
	   }
	   if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
	     wheelRNormal += 1;
	   }*/
	   wheelLeftPower = wheelLNormal*direction;
	   wheelRightPower = wheelRNormal*direction;
	   wait1Msec(10);
	 }
	 driveState = 3;
	 wheelLeftPower = wheelRightPower = -wheelBrakePower*direction;
	}
	else {
	 while (SensorValue[sensor] < value) {
	   driveState = 5;
	   /*if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
       wheelLNormal += 1;
	   }
	   if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
	     wheelRNormal += 1;
	   }*/
	   wheelLeftPower = wheelLNormal*direction;
	   wheelRightPower = wheelRNormal*direction;
	   wait1Msec(10);
	 }
	 wheelLeftPower = wheelRightPower = -wheelBrakePower*direction;
	 driveState = 3;
	 wait1Msec(20);
	}
}

void turnToLine(char sensor, int direction, int value, int power = 6, bool towards = true) {
  int prevEncL; int prevEncR;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = minMovePower + power;
	int wheelRNormal = minMovePower + power;
	if (towards) {
	 while (SensorValue[sensor] > value) {
	   driveState = 6;
	   /*if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
       wheelLNormal += 1;
	   }
	   if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
	     wheelRNormal += 1;
	   }*/
	   wheelLeftPower = wheelLNormal*direction;
	   wheelRightPower = -wheelRNormal*direction;
	   wait1Msec(10);
	 }
	 wheelLeftPower = wheelRightPower = 0;
	 driveState = 3;
	}
	else {
	 while (SensorValue[sensor] < value) {
	   driveState = 6;
	   /*if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
       wheelLNormal += 1;
	   }
	   if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
	     wheelRNormal += 1;
	   }*/
	   wheelLeftPower = wheelLNormal*direction;
	   wheelRightPower = -wheelRNormal*direction;
	   wait1Msec(10);
	 }
	 wheelLeftPower = wheelRightPower = 0;
	 driveState = 3;
	}
}

void moveToLineEL(char sensor, int direction, int value, int ticks) {
  int prevEncL; int prevEncR;
  int moveamount = ticks*direction;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = minMovePower+1;
	int wheelRNormal = minMovePower+1;
	int leftMError = 0; int rightMError = 0; int leftMOffset; int rightMOffset; int leftPowertmp; int rightPowertmp; float kp = 0.2;
	bool touched = false;
	while (!touched || (abs(moveamount - SensorValue[LeftEncoder]) > driveThreshold && abs(moveamount - SensorValue[RightEncoder]) > driveThreshold)) {
	  if (SensorValue[sensor] < value) touched = true;
		if (SensorValue[LeftEncoder] > ticks && SensorValue[LeftEncoder] > ticks) {
	    driveState = 9;
	    leftMError = moveamount - SensorValue[LeftEncoder];
	    rightMError = moveamount - SensorValue[RightEncoder];
	    leftMOffset = leftMError * kp;
	    rightMOffset = rightMError * kp;
	    if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
	      wheelLNormal += 1;
	    }
	    if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
	      wheelRNormal += 1;
	    }
	    if (abs(leftMOffset) < wheelLNormal) {
	      leftPowertmp = (leftMOffset < 0)? -wheelLNormal: wheelLNormal;
	    }
	    else if (abs(leftMOffset) > maxMovePower) {
	      leftPowertmp = (leftMOffset < 0)? -maxMovePower: maxMovePower;
	    }
	    else {
	      leftPowertmp = leftMOffset;
	    }
	    if (abs(rightMOffset) < wheelRNormal) {
	      rightPowertmp = (rightMOffset < 0)? -wheelRNormal: wheelRNormal;
	    }
	    else if (abs(rightMOffset) > maxMovePower) {
	      rightPowertmp = (rightMOffset < 0)? -maxMovePower: maxMovePower;
	    }
	    else {
	      rightPowertmp = rightMOffset;
	    }
	    wheelLeftPower = leftPowertmp;
	    wheelRightPower = rightPowertmp;
	    prevEncL = SensorValue[LeftEncoder];
	    prevEncR = SensorValue[RightEncoder];
	    wait1Msec(10);
    }
    else if (!touched) {
      wheelLeftPower = wheelLNormal*direction;
	    wheelRightPower = wheelRNormal*direction;
	    wait1Msec(10);
	  }
	}
	driveState = 3;
	wheelLeftPower = wheelRightPower = -wheelBrakePower*direction;
	wait1Msec(20);
}
